#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S2,     HTSMUX,         sensorI2CCustom)
#pragma config(Sensor, S3,     HTSPB,          sensorI2CCustom9V)
#pragma config(Sensor, S4,     color,          sensorCOLORFULL)
#pragma config(Motor,  mtr_S1_C1_1,     right,         tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     left,          tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Servo,  srvo_S1_C2_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    servo2,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoStandard)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "geary-mux-stuff.c"

int zig = left;
int zag = right;
int led = 1;

int BrightLight = 0;
int DarkLight = 0;
int LightRange = 0;

const int MotorSpeed = 15;
const int MaxWander = 2000;
const int MaxTurn = 30;

void switchZigAndZag () {
	int temp = zig; zig = zag; zag = temp;
	led = (led + 1) % 2;
  LightLed(led);
}

void CalibrateLightSensor () {
   LSsetActive(LEGOLS);

   PlaySound(soundBeepBeep);
   nxtDisplayClearTextLine(7);
   nxtDisplayTextLine(5, "Go to White");
   nxtDisplayTextLine(6, "Press [enter]");
   wait1Msec(2000);
   while (true) {
     if (nNxtButtonPressed == kEnterButton) {
       BrightLight = LSvalNorm(LEGOLS);
       break; // out of while
     }
   }
   PlaySound(soundBeepBeep);
   nxtDisplayClearTextLine(7);
   nxtDisplayTextLine(5, "Go to Dark");
   nxtDisplayTextLine(6, "Press [enter]");
   wait1Msec(2000);
   while (true) {
     if (nNxtButtonPressed == kEnterButton) {
       DarkLight = LSvalNorm(LEGOLS);
       break; // out of while
     }
   }

   PlaySound(soundFastUpwardTones);
   nxtDisplayClearTextLine(7);
   nxtDisplayTextLine(4, "White: %d", BrightLight);
   nxtDisplayTextLine(5, "Dark: %d", DarkLight);
   nxtDisplayTextLine(6, "Press [enter]");
   wait1Msec(2000);
   while (true) {
     if (nNxtButtonPressed == kEnterButton) {
       break; // out of while
     }
   }

   nxtDisplayClearTextLine(7);
   LightRange = BrightLight - DarkLight;
}

// States
//              state    color  light    meaning
#define SLOSTLINE 0   // black  dark     lost line
#define SONRIGHT  1   // white  dark     on right edge of line
#define SONLEFT   2   // black  bright   on left edge of line
#define SCENTER   3   // white  bright   on center of line

int LineState () {

   bool colorWhite = (SensorValue[color] == YELLOWCOLOR);
   bool lightBright = (BrightLight - LSvalNorm(LEGOLS) < LightRange/3);

   if (colorWhite && lightBright) return SCENTER;
   else if (colorWhite) return SONRIGHT;
   else if (lightBright) return SONLEFT;
   else return SLOSTLINE;
 }


// Transitions
//                 transition  meaning
#define TNOCHANGE     0    //    no change
#define TOFFRIGHT     1    //    off to right
#define TOFFLEFT      2    //    off to left
#define TLOST         3    //    lost
#define TBACKONRIGHT  4    //    back on from right
#define TBACKONLEFT   5    //    back on from left
#define TDRIFTLEFT    6    //    drifing to left
#define TDRIFTRIGHT   7    //    drifting to right


int DetectTransition (int oldState, int newState) {

  int result = TNOCHANGE;

	if (oldState != newState) {
		switch (newState) {
			case SLOSTLINE: result = (oldState == SONRIGHT) ? TOFFRIGHT : TOFFLEFT; break;
			case SONRIGHT : result = (oldState == SCENTER)? TDRIFTRIGHT : TBACKONRIGHT; break;
			case SONLEFT  : result = (oldState == SCENTER)? TDRIFTLEFT : TBACKONLEFT; break;
			case SCENTER  : result = (oldState == SONRIGHT)? TDRIFTLEFT : TDRIFTRIGHT; break;
		} // switch
	} // if change

	return result;
}

// Actions
#define AGOSTRAIGHT 0 // go straight
#define ATURNLEFT   1 // turn or rotate left
#define ATURNRIGHT  2 // turn or rotate right
#define ASTOP       3 // stop or turn around

int DetermineAction (int oldState, int newState, int previousAction) {
	int result = previousAction;
	int transition = DetectTransition (oldState, newState);
	switch (transition) {
		case TOFFRIGHT: result = ATURNLEFT; break;
    case TOFFLEFT : result = ATURNRIGHT; break;
    case TLOST    : result = ASTOP; break;
    case TBACKONRIGHT:
    case TBACKONLEFT:
    case TDRIFTLEFT:
    case TDRIFTRIGHT:
      result = AGOSTRAIGHT; break;
    }
  return result;
}

task main()
{
   ExtendLightServo();
   RetractLightServo();
   ExtendLightServo();
   RetractLightServo();
   ExtendLightServo();
   RetractLightServo();
   ExtendLightServo();
   RetractLightServo();


}
